#ifndef _ROBOT_WORLD_H
#define _ROBOT_WORLD_H

//forward definitions for API objects
class WorldModel;
class RobotModel;
class RobotModelLink;
class RigidObjectModel;
class TerrainModel;

//forward definitions for pointers to internal objects
struct RigidObject;
struct Environment;
struct Robot;

struct Mass
{
  double mass;        //mass
  double com[3];      //local center of mass
  double inertia[9];  //local inertia matrix
};

struct TriangleMesh
{
  std::vector<int> indices;
  std::vector<double> vertices;
};

struct ContactParameters
{
  double kFriction;
  double kRestitution;
  double kStiffness,kDamping;
};

class RobotModelLink
{
 public:
  RobotModelLink();
  int getID();
  const char* getName();
  RobotModel getRobot();
  int getParent();
  void setParent(int p);
  TriangleMesh getMesh();
  void setMesh(const TriangleMesh& mesh);
  Mass getMass();
  void setMass(const Mass& mass);
  ///Gets transformation (R,t) to the parent link
  void getParentTransform(double out[9],double out2[3]);
  void setParentTransform(const double R[9],const double t[3]);
  ///Gets the local rotational axis
  void getAxis(double out[3]);
  void setAxis(const double axis[3]);

  ///Converts point from local to world coordinates 
  void getWorldPosition(const double plocal[3],double out[3]);
  ///Converts direction from local to world coordinates 
  void getWorldDirection(const double vlocal[3],double out[3]);
  ///Converts point from world to local coordinates 
  void getLocalPosition(const double pworld[3],double out[3]);
  ///Converts direction from world to local coordinates 
  void getLocalDirection(const double vworld[3],double out[3]);
  ///Gets transformation (R,t) to the world frame
  void getTransform(double out[9],double out2[3]);
  ///Sets transformation (R,t) to the world frame.  Note: this does NOT
  ///perform inverse kinematics.  The transform is overwritten when the
  ///robot's setConfig() method is called.
  void setTransform(const double R[9],const double t[3]);
  ///Returns the total jacobian of the local point p (row-major matrix)
  ///(orientation jacobian is stacked on position jacobian)
  void getJacobian(const double p[3],std::vector<std::vector<double> >& out);
  ///Returns the jacobian of the local point p (row-major matrix)
  void getPositionJacobian(const double p[3],std::vector<std::vector<double> >& out);
  ///Returns the orientation jacobian of the link (row-major matrix)
  void getOrientationJacobian(std::vector<std::vector<double> >& out);
  void drawLocalGL(bool keepAppearance=true);
  void drawWorldGL(bool keepAppearance=true);

  int world;
  int robotIndex;
  Robot* robot;
  int index;
};

class RobotModel
{
 public:
  RobotModel();
  int getID();
  const char* getName();
  int numLinks();
  RobotModelLink getLink(int index);
  RobotModelLink getLink(const char* name);

  //kinematic and dynamic properties
  void getConfig(std::vector<double>& out);
  void getVelocity(std::vector<double>& out);
  void setConfig(const std::vector<double>& q);
  void setVelocity(const std::vector<double>& dq);
  void getJointLimits(std::vector<double>& out,std::vector<double>& out2);
  void setJointLimits(const std::vector<double>& qmin,const std::vector<double>& qmax);
  void getVelocityLimits(std::vector<double>& out);
  void setVelocityLimits(const std::vector<double>& vmax);
  void getAccelerationLimits(std::vector<double>& out);
  void setAccelerationLimits(const std::vector<double>& amax);
  void getTorqueLimits(std::vector<double>& out);
  void setTorqueLimits(const std::vector<double>& tmax);

  //dynamics functions
  void getMassMatrix(std::vector<std::vector<double> >& B);
  void getMassMatrixInv(std::vector<std::vector<double> >& Binv);
  void getCoriolisForceMatrix(std::vector<std::vector<double> >& C);
  void getCoriolisForces(std::vector<double>& C);
  void getGravityForces(const double g[3],std::vector<double>& G);
  void torquesFromAccel(const std::vector<double>& ddq,std::vector<double>& out);
  void accelFromTorques(const std::vector<double>& t,std::vector<double>& out);

  //geometry functions
  bool selfCollisionEnabled(int link1,int link2);
  void drawGL(bool keepAppearance=true);

  int world;
  int index;
  Robot* robot;
};

class RigidObjectModel
{
 public:
  RigidObjectModel();
  int getID();
  const char* getName();
  TriangleMesh getMesh();
  void setMesh(const TriangleMesh& mesh);
  Mass getMass();
  void setMass(const Mass& mass);
  ContactParameters getContactParameters();
  void setContactParameters(const ContactParameters& params);
  void getTransform(double out[9],double out2[3]);
  void setTransform(const double R[9],const double t[3]);
  void drawGL(bool keepAppearance=true);

  int world;
  int index;
  RigidObject* object;
};

class TerrainModel
{
 public:
  TerrainModel();
  int getID();
  const char* getName();
  TriangleMesh getMesh();
  void setMesh(const TriangleMesh& mesh);
  void setFriction(double friction);
  void drawGL(bool keepAppearance=true);

  int world;
  int index;
  Environment* terrain;
};

class WorldModel
{
 public:
  WorldModel();
  WorldModel(int index);
  WorldModel(const WorldModel& w);
  ~WorldModel();
  const WorldModel& operator = (const WorldModel& w);
  bool readFile(const char* fn);
  int numRobots();
  int numRobotLinks(int robot);
  int numRigidObjects();
  int numTerrains();
  int numIDs();
  RobotModel robot(int index);
  RobotModel robot(const char* name);
  RobotModelLink robotLink(int robot,int index);
  RobotModelLink robotLink(const char* robot,const char* name);
  RigidObjectModel rigidObject(int index);
  RigidObjectModel rigidObject(const char* name);
  TerrainModel terrain(int index);
  TerrainModel terrain(const char* name);
  RobotModel makeRobot(const char* name);
  RigidObjectModel makeRigidObject(const char* name);
  TerrainModel makeTerrain(const char* name);
  void drawGL();

  //WARNING: do not modify this member directly
  int index;
};

#endif
